import math


class LonLatAndXY:

    def ellipsoid_pars(self, ell='wgs84'):
        # 添加任何椭球体的 "a" 和 "f" 参数
        if ell == 'wgs84':
            # 长半轴
            a = 6378137.0
            f = 1/298.257223563

        elif ell == 'CGCS2000':
            a = 6378137.0
            f = 1 / 298.257222101
        else:
            raise Exception('没有这个椭球体')
        # 短半轴
        b = a * (1 - f)
        # 第一偏心率
        e = math.sqrt(a**2-b**2)/a
        # 第二偏心率
        e2 = math.sqrt(a**2-b**2)/b
        return a, b, e, e2

    def central_meridian_diff(self, lon, N):
        """
        计算经度对应的中央子午线差异和地理带号。

        参数：
            lon (float): 输入的经度
            N (int) : 输入带号，3或6

        返回：
            中央子午线的经度
        """
        if N == 3:
            # 计算地理带号(3度带)
            n = round(lon/3)
            # 计算对应中央子午线的经度
            lon0 = n*3
        elif N == 6:
            # 计算地理带号(6度带)
            n = math.trunc(lon / 6) + 1
            # 计算对应中央子午线的经度
            lon0 = n * 6 - 3
        else:
            raise ValueError("参数值必须为3或6")
        return lon0

    def gps84ToXY(self, L, B, degree, withBand=False):
        """
        WGS84经纬度转CGCS2000投影坐标(UTM投影)
            L: 经度
            B: 纬度
            degree: 分度带, 3或6度带
        return:
            投影坐标 x, y
        """
        # WGS84 椭球体参数
        a, b, e, e2 = self.ellipsoid_pars('wgs84')
        # 确定带号和中央子午线经度
        lon0 = self.central_meridian_diff(L, degree)
        # 计算中央子午线差异（弧度）
        deltaL = (L - lon0) / 180 * math.pi
        # 将角度转换为弧度
        radB = B * math.pi / 180
        # 卯酉圈半径（Ns）
        N = a * a / b / math.sqrt(1 + e2 * e2 *
                                  math.cos(radB) * math.cos(radB))
        # 5系数
        C1 = 1.0 + 3.0 / 4 * e * e + 45.0 / 64 * e ** 4 + \
            175.0 / 256 * e ** 6 + 11025.0 / 16384 * e ** 8
        C2 = 3.0 / 4 * e * e + 15.0 / 16 * e ** 4 + \
            525.0 / 512 * e ** 6 + 2205.0 / 2048 * e ** 8
        C3 = 15.0 / 64 * e ** 4 + 105.0 / 256 * e ** 6 + 2205.0 / 4096 * e ** 8
        C4 = 35.0 / 512 * e ** 6 + 315.0 / 2048 * e ** 8
        C5 = 315.0 / 131072 * e ** 8
        # 纬度切线函数值
        t = math.tan(radB)
        eta = e2 * math.cos(radB)
        X = a * (1 - e * e) * (
            C1 * radB - C2 * math.sin(2 * radB) / 2 + C3 * math.sin(4 * radB) / 4 -
            C4 * math.sin(6 * radB) / 6 + C5 * math.sin(8 * radB))

        # 计算 X 坐标
        x = X + N * math.sin(radB) * math.cos(radB) * deltaL ** 2 * (
            1 + deltaL ** 2 * math.cos(radB) ** 2 * (5 - t ** 2 + 9 * eta ** 2 + 4 * eta ** 4) / 12 +
            deltaL ** 4 * math.cos(radB) ** 4 * (61 - 58 * t ** 2 + t ** 4) / 360) / 2

        # 计算 Y 坐标
        y = N * deltaL * math.cos(radB) * (
            1 + deltaL ** 2 * math.cos(radB) ** 2 * (1 - t ** 2 + eta ** 2) / 6 +
            deltaL ** 4 * math.cos(radB) ** 4 * (5 - 18 * t ** 2 + t ** 4 - 14 * eta ** 2 - 58 * eta ** 2 * t ** 2) / 120) + 500000

        return x, y

    def xyTowgs84(self, x, y, L0):
        """
        WGS84经纬度转CGCS2000投影坐标(UTM投影)
            x: 投影坐标 x
            y: 投影坐标 y
            L0: 中央子午线经度
        return:
            lon(经度), lat(纬度)
        """
        # CGCS2000 椭球体参数
        a, b, e, e2 = self.ellipsoid_pars('CGCS2000')
        ef = e**2  # 第一偏心率的平方
        e2f = e2**2  # 第二偏心率的平方

        # 主曲率计算
        m0, m2, m4, m6, m8 = 0, 0, 0, 0, 0
        m0 = a * (1 - ef)
        m2 = 3.0 / 2.0 * ef * m0
        m4 = ef * m2 * 5.0 / 4.0
        m6 = ef * m4 * 7.0 / 6.0
        m8 = ef * m6 * 9.0 / 8.0

        # 子午线曲率计算
        a0, a2, a4, a6, a8 = 0, 0, 0, 0, 0
        a0 = m0 + m2 / 2.0 + m4 * 3.0 / 8.0 + m6 * 5.0 / 16.0 + m8 * 35.0 / 128.0
        a2 = m2 / 2.0 + m4 / 2.0 + m6 * 15.0 / 32.0 + m8 * 7.0 / 16.0
        a4 = m4 / 8.0 + m6 * 3.0 / 16.0 + m8 * 7.0 / 32.0
        a6 = m6 / 32.0 + m8 / 16.0
        a8 = m8 / 128.0

        y = y - 500000
        X = x
        FBf = 0
        Bf0, Bf1 = X / a0, 0

        # 计算 Bf 的值，直到满足条件
        while abs(Bf0 - Bf1) >= 0.0001:
            Bf1 = Bf0
            FBf = -a2 * math.sin(2 * Bf0) / 2 + a4 * math.sin(4 * Bf0) / \
                4 - a6 * math.sin(6 * Bf0) / 6 + a8 * math.sin(8 * Bf0) / 8
            Bf0 = (X - FBf) / a0
        Bf = Bf0

        # 计算公式中参数
        # 卯酉圈曲率半径
        Wf = math.sqrt(1 - ef * math.sin(Bf) * math.sin(Bf))
        # 子午线曲率半径
        Nf = a / Wf
        # 子午线弯曲半径
        Mf = a * (1 - ef) / (Wf ** 3)
        # 第二偏心率平方
        nffang = e2f * math.cos(Bf) * math.cos(Bf)
        # 切线函数
        tf = math.tan(Bf)
        B = Bf - tf * y ** 2 / (2 * Mf * Nf) + tf * (
            5 + 3 * tf ** 2 + nffang - 9 * nffang * tf ** 2) * y ** 4 / (24 * Mf * Nf ** 3) - tf * (
            61 + 90 * tf ** 2 + 45 * tf ** 4) * y ** 6 / (720 * Mf * Nf ** 5)
        l = y / (Nf * math.cos(Bf)) - (1 + 2 * tf ** 2 + nffang) * y ** 3 / (6 * Nf ** 3 * math.cos(Bf)) + (
            5 + 28 * tf ** 2 + 24 * tf ** 4) * y ** 5 / (120 * Nf ** 5 * math.cos(Bf))
        # 中央子午线经度转弧度
        deg = L0 / 180 * math.pi
        L = l + deg

        # 转化成为十进制经纬度格式
        array_L = self.rad2dms(L)
        array_B = self.rad2dms(B)
        lon = self.dms2dec(array_L)
        lat = self.dms2dec(array_B)

        return lon, lat

    def rad2dms(self, rad):
        # 将弧度转化为度分秒
        a = [0, 0, 0]
        dms = rad * 180.0 / math.pi * 3600.0
        a[0] = int(dms / 3600.0)
        a[1] = int((dms - a[0] * 3600) / 60.0)
        a[2] = int(dms - a[0] * 3600 - a[1] * 60)
        return a

    def dms2dec(self, dms):
        # 将度分秒转化为十进制坐标
        dec = 0.0
        dec = dms[0] + dms[1] / 60.0 + dms[2] / 3600.0
        return dec


if __name__ == '__main__':
    # 创建 LonLatAndXY 的实例
    lon_lat_xy_converter = LonLatAndXY()

    # 示例经纬度坐标
    L, B = 116.40492, 39.91457
    degree = 3

    # 调用 gps84ToXY 方法将 WGS84 经纬度转换为大地2000坐标
    x, y = lon_lat_xy_converter.gps84ToXY(L, B, degree)
    print("大地2000坐标 (Y, X):", y, x)

    # 调用 xyTowgs84 方法将大地2000坐标转换为 WGS84 经纬度
    y, x = 449120.3896817744, 4420212.950850243
    # 获取中央经线
    L0 = lon_lat_xy_converter.central_meridian_diff(L, degree)
    lon, lat = lon_lat_xy_converter.xyTowgs84(x, y, L0)
    print("WGS84 经纬度 (纬度, 经度):", lon, lat)
    # 449120.3896817758 4420212.950711163 正确的
    # 449120.3896823084 4420212.950824771  很接近
